/*! \file completefuruta.cpp

\batwing dynamics model: Euler-Lagrange equations of motion

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.

*/


/*


INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param double TAU: Torque input

OUTPUT

\param VEC out: State modified.

*/

#include "../../dynlib.h"

void dynmodel(Matrix &SimParam,Matrix &X,int i,double Tau,ColumnVector &out)
{
	/*batwing model
	batwing(SimParam,teta,dteta,d2teta,i,Tau)
	m=SimParam[1,1]	
	r=SimParam[1,3]
	L=SimParam[1,4]
	I=SimParam[1,5]
	bs=SimParam[1,6]
	grav=SimParam[3,5]*/
	
	double temp, temp1, temp2, dX2, dx3;
	//temp calc
	//temp=L*(I+m*r**2*(1-cos(teta[1,i])**2))
	temp=SimParam(1,4)*(SimParam(1,5)+SimParam(1,1)*SimParam(1,3)*SimParam(1,3)*(1-cos(X(2,i))*cos(X(2,i))));
		
	//temp1=0.5*m*r*L*dteta[1,i]*dteta[0,i]*sin(teta[1,i])+m*g*L*sin(teta[1,i])+bs*dteta[1,i]
	temp1=0.5*SimParam(1,1)*SimParam(1,3)*SimParam(1,4)*X(4,i)*X(3,i)*sin(X(2,i))+SimParam(1,1)*SimParam(3,5)*SimParam(1,4)*sin(X(2,i))+SimParam(1,6)*X(4,i);
	
	//temp2=(0.5*m*r*L*dteta[1,i]**2*sin(teta[1,i])+Tau)
	temp2=(0.5*SimParam(1,1)*SimParam(1,3)*SimParam(1,4)*X(4,i)*X(4,i)*sin(X(2,i))+Tau);
	
	//d2teta[:] calc
	//d2teta[1,i]=(I+m*r**2)*(temp1)/(m*L*temp)-r*cos(teta[1,i])*(temp2)/temp
	out(4)=(SimParam(1,5)+SimParam(1,1)*SimParam(1,3)*SimParam(1,3))*(temp1)/(SimParam(1,1)*SimParam(1,4)*temp)-SimParam(1,3)*cos(X(2,i))*(temp2)/temp;
	
	//d2teta[0,i]=L*(temp2)/(temp)-r*cos(teta[1,i])*(temp1)/temp
	out(3)=SimParam(1,4)*(temp2)/(temp)-SimParam(1,3)*cos(X(2,i))*(temp1)/temp;
	
	out(1)=X(3,i);
	
	out(2)=X(4,i);
}
